import math
import pygame
from udp import RobotConnection
from gamepads.gamepad import Gamepad
from settings import *

# Shapes and Colors
BLACK	= (0x00, 0x00, 0x00)
GREY_5	= (0xf2, 0xf2, 0xf2)

class Main:
	# Static Rectangles
	staticRects = list()
	ConnectionBoundingBox	= pygame.Rect(16,338,416,208)
	TerminalBoundingBox	= pygame.Rect(448,338,544,208)
	staticRects.append(ConnectionBoundingBox)
	staticRects.append(TerminalBoundingBox)

	def __init__(self):
		self.SCREEN_WIDTH = 1008
		self.SCREEN_HEIGHT = 560
		self.screen = pygame.display.set_mode((self.SCREEN_WIDTH,self.SCREEN_HEIGHT))

		# Init pygame.joystick
		pygame.joystick.init()

		self.gamepads = []
		
		# Network Connection Init
		self.robot = RobotConnection("10.0." + str(TEAM_NUMBER) + ".2")

	def setupGame(self):
		self.clock = pygame.time.Clock()
		self.FPS = 25
		
		# Get the number of joysticks
		numGamepads = pygame.joystick.get_count()
		
		# We only support 1 or 2 gamepads
		self.gamepad0 = Gamepad(0,16,16)
		self.gamepads.append(self.gamepad0)
		if numGamepads == 2:
			self.gamepad1 = Gamepad(1,self.gamepad0.LEFT+self.gamepad0.WIDTH+16, self.gamepad0.TOP)
			self.gamepads.append(self.gamepad1)

	def runGame(self):
		self.gameRunning = 1

		while self.gameRunning:
			self.getInput()
			self.compute()
			self.sendCommand()
			self.draw()
			self.clock.tick(self.FPS)

	def getInput(self):
		for event in pygame.event.get():
			if event.type == pygame.QUIT:
				self.gameRunning = 0

		
		#pygame.display.set_caption(str(self.joy.x) + ', ' + str(self.joy.y) + ', ' + str(self.joy.rad))

	def draw(self):
		self.screen.fill((255,255,255))
		
		# Draw Static Rectangles
		for thisRect in Main.staticRects:
			pygame.draw.rect(self.screen, BLACK, thisRect, 2)

		for o in self.gamepads:
			o.draw(self.screen)

		pygame.display.flip()

	def compute(self):
		i = 0
		while i < len(self.gamepads):
			self.gamepads[i].compute()
			i += 1
	
	def sendCommand(self):
		# Read input from the gamepad(s)
		self.robot.setAnalog(0,self.gamepads[0].x0*127 + 127)
		self.robot.setAnalog(1,self.gamepads[0].y0*-127 + 127)
		self.robot.setAnalog(2,self.gamepads[0].x1*127 + 127)
		self.robot.setAnalog(3,self.gamepads[0].y1*-127 + 127)
		
		if (len(self.gamepads) == 2):
			self.robot.setAnalog(4,self.gamepads[1].x0*127 + 127)
			self.robot.setAnalog(5,self.gamepads[1].y0*-127 + 127)
			self.robot.setAnalog(6,self.gamepads[1].x1*127 + 127)
			self.robot.setAnalog(7,self.gamepads[1].y1*-127 + 127)
		
		offset = 0
		for i in xrange(len(self.gamepads)):
			for j in xrange(self.gamepads[i].numButtons):
				self.robot.setDigital(j + offset, 
						      self.gamepads[i].buttons[j])

		# Send the packet
		self.robot.sendPacket()


m = Main()
m.setupGame()
m.runGame()

pygame.quit()
